/// @file ObstacleManager.cpp
/// @brief the manager of obstacle, creates, draws and maintain them

#include "ObstacleManager.h"
#include "Configuration.h"
#include "Utilities.h"

ObstacleManager::ObstacleManager()
{
    //create obstacles from config
    Configuration::initialiseObstacles
                            (
                                m_cubeEnabled,
                                m_cubePosition,
                                m_cubeDimension,
                                m_sphereEnabled,
                                m_spherePosition,
                                m_sphereRadius
                            );

    //create bbox for cube
    m_cubeBBox = new ngl::BBox(m_cubePosition, m_cubeDimension.m_x * 1.1, m_cubeDimension.m_y * 1.1, m_cubeDimension.m_z * 1.1);

    m_obstacleEnabled = true;
    m_drawBBox = true;
}

ObstacleManager::~ObstacleManager()
{
    //clean up
    std::cout << "Obstacle Manager Cleanup" << std::endl;

    delete m_cubeBBox;
}

void ObstacleManager::loadGeometry()
{
    ngl::VBOPrimitives *prim=ngl::VBOPrimitives::instance();
    prim->createVBOSphere("obstacleSphere", 1, 30);
}

void ObstacleManager::draw
                    (
                        ngl::TransformStack _txStack,
                        ShaderManager* io_sman,
                        const std::string _shader
                    )
{
    ngl::VBOPrimitives *prim=ngl::VBOPrimitives::instance();

    //draw obstacles if enabled
    if (m_obstacleEnabled)
    {
        //draw cube if enabled
        if (m_cubeEnabled)
        {
            _txStack.pushTransform();
            {
                _txStack.getCurrentTransform().setPosition(m_cubePosition.m_x, m_cubePosition.m_y, m_cubePosition.m_z);
                _txStack.getCurrentTransform().setScale(m_cubeDimension.m_x, m_cubeDimension.m_y, m_cubeDimension.m_z);

                io_sman->updateModelColor(_shader, _txStack.getCurrAndGlobal().getMatrix(), ngl::Colour(0, 1, 0));
                prim->draw("cube");
            }
            _txStack.popTransform();

            if (m_drawBBox)
            {
                _txStack.pushTransform();
                {
                    io_sman->updateModelColor("Colour", _txStack.getCurrAndGlobal().getMatrix(), ngl::Colour(0, 1, 1));
                    m_cubeBBox->draw();
                }
                _txStack.popTransform();
            }

        }

        //draw sphere if enabled
        if (m_sphereEnabled)
        {
            _txStack.pushTransform();
            {
                _txStack.getCurrentTransform().setPosition(m_spherePosition.m_x, m_spherePosition.m_y, m_spherePosition.m_z);
                _txStack.getCurrentTransform().setScale(m_sphereRadius, m_sphereRadius, m_sphereRadius);

                io_sman->updateModelColor(_shader, _txStack.getCurrAndGlobal().getMatrix(), ngl::Colour(1, 0, 0));
                prim->draw("obstacleSphere");
            }
            _txStack.popTransform();
        }

    }
}

bool ObstacleManager::checkAndResolveCollision
                                (
                                    ngl::Vector currentPosition,
                                    ngl::Vector lastPosition,
                                    float currentParticleRadius,
                                    ngl::Vector &newPosition
                                )
{
    if (m_obstacleEnabled)
    {
        bool collisionWithSphere = !m_sphereEnabled ? false : isObjectInsideSphere(m_spherePosition, m_sphereRadius, currentPosition, currentParticleRadius, newPosition);
        if (collisionWithSphere) return collisionWithSphere;

        bool collisionWithCube = !m_cubeEnabled ? false : isObjectInsideCube(m_cubeBBox, currentPosition, lastPosition, newPosition);
        return collisionWithCube;
    }
    return false;

}

bool ObstacleManager::isObjectInsideCube
                                (
                                    ngl::BBox* cubeBBox,
                                    ngl::Vector currentPosition,
                                    ngl::Vector lastPosition,
                                    ngl::Vector &newPosition
                                )
{
    //determine bounds of box
    float xMax = cubeBBox->center().m_x + (cubeBBox->width()/2.0f);
    float xMin = cubeBBox->center().m_x - (cubeBBox->width()/2.0f);
    float yMax = cubeBBox->center().m_y + (cubeBBox->height()/2.0f);
    float yMin = cubeBBox->center().m_y - (cubeBBox->height()/2.0f);
    float zMax = cubeBBox->center().m_z + (cubeBBox->depth()/2.0f);
    float zMin = cubeBBox->center().m_z - (cubeBBox->depth()/2.0f);

    //assumes collision
    bool isColliding = true;

    //check if sphere is inside the box
    if (currentPosition.m_x > xMax) isColliding = false;
    if (currentPosition.m_x < xMin) isColliding = false;
    if (currentPosition.m_y > yMax) isColliding = false;
    if (currentPosition.m_y < yMin) isColliding = false;
    if (currentPosition.m_z > zMax) isColliding = false;
    if (currentPosition.m_z < zMin) isColliding = false;

    if (isColliding)
    {
        newPosition.set(currentPosition);

        //collision occured -> determine the face of the box that caused collision

        if ((lastPosition.m_y >= yMax) && (currentPosition.m_y < yMax))
        {
            //check against top face -> move particle to surface of top face
            newPosition.m_y = yMax;
        }
        else if ((lastPosition.m_y <= yMin) && (currentPosition.m_y > yMin))
        {
            //check against bottom face -> move particle to surface of bottom face
            newPosition.m_y = yMin;
        }
        else if ((lastPosition.m_x >= xMax) && (currentPosition.m_x < xMax))
        {
            //check against left face -> move particle to surface of left face
            newPosition.m_x = xMax;
        }
        else if ((lastPosition.m_x <= xMin) && (currentPosition.m_x > xMin))
        {
            //check against right face -> move particle to surface of right face
            newPosition.m_x = xMin;
        }
        else if ((lastPosition.m_z >= zMax) && (currentPosition.m_z < zMax))
        {
            //check against outplane face -> move particle to surface of outplane face
            newPosition.m_z = zMax;
        }
        else if ((lastPosition.m_z <= zMin) && (currentPosition.m_z > zMin))
        {
            //check against outplane face -> move particle to surface of outplane face
            newPosition.m_z = zMin;
        }
    }

    return isColliding;
}

bool ObstacleManager::isObjectInsideSphere
                                (
                                    const ngl::Vector _obstacleCenter,
                                    const float _obstacleRadius,
                                    const ngl::Vector _objectCenter,
                                    const float _objectRadius,
                                    ngl::Vector &o_safePosition
                                )
{
    //assume no collision by default
    bool isColliding = false;

    //get vector from obstacle center through particle's position
    ngl::Vector vector = _objectCenter - _obstacleCenter;

    //get length of separation between the 2 objects
    float separationDistance = vector.length();

    //calculate the safe distance between the 2 objects
    float safeDistance = _obstacleRadius + _objectRadius;

    //evaluate whether collision occured
    isColliding = (separationDistance <= safeDistance);

    if (isColliding)
    {
        //collision occured -> determine safe position outside of sphere

        //normalise normal vector
        ngl::Vector targetVector = Utilities::normaliseVector(vector, vector);

        //calculate the safe position outside of the obstacle
        o_safePosition = (targetVector * safeDistance) + _obstacleCenter;
    }

    return isColliding;
}

